//You can include any C libraries that you normally use
#include "math.h"
#include "mex.h"   //--This one is required
#include "matrix.h"


#define himg0(i,j,k) himg0[(i+j*ix+k*ix*iy)] 
#define himg(i,j,k) himg[(i+j*lenx+k*lenx*leny)] 

#define img0(i,j,k) img0[(i+j*ix+k*ix*iy)] 
#define img(i,j,k) img[(i+j*lenx+k*lenx*leny)]
#define newimg(i,j,k) newimg[(i+j*ix+k*ix*iy)]
#define tempimg(i,j,k) tempimg[(i+j*ix+k*ix*iy)]

#define newimg0(i,j) newimg0[i+j*ix]
#define newimg1(i,j) newimg1[i+j*ix]
#define newimg2(i,j) newimg2[i+j*ix]

#define gimg(i,j) gimg[i+j*ix]
#define out(i,j) out[i+j*ix]


double HistDistance(double* img, int xstart, int xend, int ystart, int yend, double rm, double gm, double bm, 
        double rstd, double gstd, double bstd, int lenx, int leny)
{                   
    int i, j; 
        
    double r, g, b, r1, g1, b1;
    double dist, d1, d2, d3;        
    
    double totalpixels = abs(xend-xstart)*abs(yend-ystart);
    
    dist = 10000;
    
    if(totalpixels > 0)
    {
        
        for(i = xstart; i < xend; i++)
        {
            for(j = ystart; j < yend; j ++)
            {                       
                r = (int)img(i,j,0);
                g = (int)img(i,j,1);
                b = (int)img(i,j,2);     
                                                    
                r1 += r;
                g1 += g;
                b1 += b;
            }
        }
        
        
        // calculate center of cluster (= mean of each color)
        r1 = r1/totalpixels;
        g1 = g1/totalpixels;
        b1 = b1/totalpixels;  
    
        d1 = rm-r1;
        d2 = gm-g1;
        d3 = bm-b1;
        
        if(fabs(d1) < rstd && fabs(d2) < gstd && fabs(d3) < bstd)
        {    
            dist = sqrt(d1*d1+d2*d2+d3*d3);             
        }
    }
    
    return dist;
}





void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
               
    double* img0 = mxGetPr(prhs[0]); //template
    double* img = mxGetPr(prhs[1]); //main image


    const int *sz = mxGetDimensions(prhs[0]); 
    int ix = sz[0];
    int iy = sz[1];
    int iz = sz[2];

    const int *szmain = mxGetDimensions(prhs[1]); 
    int lenx = szmain[0];
    int leny = szmain[1];

    int x, y, x1, x2, y1, y2;

    int x1best = 0;
    int x2best = 0;
    int y1best = 0;
    int y2best = 0;

    double bestscore = 100000;
    double score;

    double r,g, b, r0, g0, b0;
    double d1, d2, d3;
    int i, j, k;

    double dist;

    int lenx2, leny2, npixels;

    int bestremoved, nremoved, nstep;

    int testmax = 0;
    int testmax2 = 0;

    double besth = 10000;

    double div = ix*iy;

    lenx2 = lenx - ix - 1;
    leny2 = leny - iy - 1;

    y1 = 0;
    bestremoved = 0; 
    nremoved = 0;
    bestscore = ix*iy*iz*10000;

    nstep = 1;
    dist = 0;
       
    for(x = 0; x < ix; x++) //for each point in template subtract from image: ix = xlength of template
    {                
        for(y = 0; y < iy; y++) //iy = ylength of template
        {                                                                                                        
              r = img(x,y,0);        
              g = img(x,y,1);        
              b = img(x,y,2);                                                                
        
              r0 = img0(x,y,0);        
              g0 = img0(x,y,1);
              b0 = img0(x,y,2);                                       
                                      
                    
              d1 = r0-r;
              d2 = g0-g;
              d3 = b0-b;                                        
                    
              dist = dist+sqrt(d1*d1+d2*d2+d3*d3);  
                           
        }
     }                         
       
     score = dist;
      
     bestscore = score;
     bestremoved = nremoved;                                
                                                                                                             
    plhs[0] = mxCreateDoubleScalar(x1best);
    plhs[1] = mxCreateDoubleScalar(x2best);
    plhs[2] = mxCreateDoubleScalar(y1best);
    plhs[3] = mxCreateDoubleScalar(y2best);
    plhs[4] = mxCreateDoubleScalar(bestscore);
    plhs[5] = mxCreateDoubleScalar(bestremoved);
 
    return;
}